www.gusucode.com > matlab编程多天线姿态确定工具箱，包括星历读取、单点定位、差分定位、坐标转换、定姿 > matlab编程多天线姿态确定工具箱，包括星历读取、单点定位、差分定位、坐标转换、定姿/A MATLAB toolbox for attitude determination with GPS multi-antenna systems/AttDet_16_3_2009/AttDet/mainpro_ph.m
%%========================================
%% Toolbox for attitude determination
%% Zhen Dai
%% dai@zess.uni-siegen.de
%% ZESS, University of Siegen, Germany
%% Last Modified : 1.Sep.2008
%%========================================
%% Functions:
%% Main program for attitude determination using carrier phase.
%% Input parameters:
%% ------- From data file DataSatRecord ------:
%% mCommonEpoch
%% -> Synchronized epochs for all antennas
%% mEpochStr
%% -> Epochs expressed in string
%% mMeasurement
%% -> Code data, phase data, satellite visibility
%% maskangle
%% -> Satellite elevation mask angle (cut-off angle)
%% smoothing_interval
%% -> Smoothing inteval, i.e. the length of accumulated data
%% for HATCH smoothing
%% mBaseline
%% -> Magnitude of the baselines
%%
%% ------- From data file RinexNav ------:
%% mEphemerides
%% -> Ephemerides parameters needed for the calculation of
%% satellite positions and the acquirement of some
%% corrections
%% mEphTime
%% -> Time tag of each ephemerides. It is used to search for the
%% proper ephemerides for a specific satelilte at a specific time
%% ------- From mat file "ambiguity" ------:
%% mAmbiguity
%% -> Double-differenced phase ambiguities of each antenna%%
%% Output:
%% mAttitudeRecordLSQLinear
%% -> Attitude parameters from least squares estimation
%% mAttitudeRecordDirect
%% -> Attitude parameters from direct attitude computation
%% Remarks:
%% 1. Result will be illustrated and also saved into the data file
%% "Results.txt"
clear all
clc
%% ********************************************************************
%% Constant Definition
%% ********************************************************************
sol=299792458; %% Speed of light
freq1=1575.42e6; %%frequency of L1
lambda1=sol/(freq1);%% lambda on L1
totalGPSsatellite=32; %% Assuming that there are maximal 32 GPS satellites
%% Some global variables for the ephemerides
global mEphTime mEphemerides
%% ********************************************************************
%% Load Measurement
%% ********************************************************************
load DataSatRecord
load RinexNav
load ambiguity
%% ********************************************************************
%% Initialization
%% ********************************************************************
totalepoch=size(mCommonEpoch,1); %% how many synchonized epochs to be proessed
totalantenna=size(mMeasurement,1); % how many antenna/receiver pairs
%% If baselines are identified, then extract them.
if num_baseline~=0
% Baseline 1-2 1-3 2-3
b12=mBaseline(1,1);
b13=mBaseline(2,1);
b23=mBaseline(3,1);
% Baseline 1-4 2-4 3-4, 1-5 2-5 3-5, 1-6 2-6 3-6
mBaselineAdd=[];
for antid=4:1:totalantenna,
mBaselineAdd(antid-3,1:3)= mBaseline(antid,1:3);
end
else
%% if no baseline given, then we consider the first 3 antennas
totalantenna=3;
end
%% some matrix for recording the estimated attitude parameters
%% 1st epoch will be deleted
mAttitudeRecordDirect=zeros(totalepoch-1,3);
mAttitudeRecordLSQMatrix=zeros(totalepoch-1,3);
mAttitudeRecordLSQLinear=zeros(totalepoch-1,3);
%% Initilize the matrix recording measurement
mPhase=zeros(totalantenna,totalGPSsatellite,totalepoch);
mCode=zeros(totalantenna,totalGPSsatellite,totalepoch);
mCodeSmoothed=zeros(totalantenna,totalGPSsatellite,totalepoch);
mSat=zeros(totalantenna,totalGPSsatellite,totalepoch);
mSatPos=zeros(totalantenna,totalGPSsatellite,totalepoch,3);
vKeySat=zeros(totalepoch,1);
%% ********************************************************************
%% Data extraction
%% ********************************************************************
%% Extract the measurement and satellite information
%% For each matrix including phase , code, and sat visibility:
%% 1st dimension : receiver ID( 1~n), No.1 is master antenna
%% 2nd dimension: satelite ID (1~totalGPSsatellite of 32)
%% 3rd dimension : epoch (1~totalepoch)
%% ********************************************************************
for epoch=1:1:totalepoch,%% Loop for epochs
for antid=1:1:totalantenna, %% Loop for receiver
%% Satellites visibility. 1=visible
mSat(antid,1:totalGPSsatellite,epoch)=squeeze(squeeze(mMeasurement(antid,epoch,1,:)));
for k=1:1:totalGPSsatellite, %% Loop for satelltes of current epoch
if mSat(antid,k,epoch)==1,
mPhase(antid,k,epoch)=mMeasurement(antid,epoch,3,k);%% Phase data
mCode(antid,k,epoch)=mMeasurement(antid,epoch,2,k); %% Code range without smoothing
end
end
end
end
%% ********************************************************************
%% Smoothing code by carrier phase
%% ********************************************************************
mCodeSmoothed=zeros(totalantenna,totalGPSsatellite,totalepoch);
for i=1:1:totalantenna,
clear mCodeTemp
mRawCode=squeeze(mCode(i,:,:));
mRawPhase=squeeze(mPhase(i,:,:));
mCodeSmoothed(i,:,:)=Smoothing(mRawCode,mRawPhase,smoothing_interval);
end
%% ********************************************************************
%% Single point positioning for master antenna
%% ********************************************************************
waitbar_sp = waitbar(0,'Single point positioning......');
%% Satellite clock correction
vSatClockCor=zeros(1, totalGPSsatellite);
%% Satellite position in ECEF
mSatPos=zeros(totalantenna,totalepoch,totalGPSsatellite,3);
for epoch = 1:1:totalepoch, %%Loop for epoch
mSatPosCur=zeros(totalGPSsatellite,3); %% Satellite positions of current epoch
vCodeCur=zeros(1,totalGPSsatellite); %% Code data of current epoch
vPhaseCur=zeros(1,totalGPSsatellite); %% Phase data of current epoch
vCodeSmoothedCur=zeros(1,totalGPSsatellite); %% Smoothed code data of current epoch
clear vSatUsed vElevationAngle
%% Extract the satellite info and measurement for master antenna
vSatIDs=find(squeeze(mSat(1,1:totalGPSsatellite,epoch))~=0);%% visible satellites PRNs of current epoch
vCodeCur=mCode(1,:,epoch);
vPhaseCur=mPhase(1,:,epoch);
vCodeSmoothedCur=mCodeSmoothed(1,:,epoch);
%% Initialization for the first epoch
if epoch==1,
vInitPos=[1 1 1]; %% The initial guess of adjustment
receiver_clock=0; %% Receiver clock error
vEpheEopchs=zeros(totalGPSsatellite,1); %% Index of ephemerides paragrath used
vGroupDelay=zeros(1, totalGPSsatellite); %% Group delay error
vSatClockCor=zeros(1, totalGPSsatellite);%% Satellite clock error
else
%% if not the first epoch, the LSQ adjustment starts from the position of last epoch
vInitPos(1:3)=mAntXYZAll(1,epoch-1,1:3);
end
time=mCommonEpoch(epoch); %% "Second of the week " of current epoch
%% Calculate the coordinate of satellites
for i=1:1:length(vSatIDs),
satid=vSatIDs(i); %% Satellite PRN
%% Find a proper ephemerides
vEpheEopchs(satid)=SeekEpheEpoch(time,satid);
%% Estimate the transit time of GPS signal.
transit_time=vCodeCur(satid)/sol+vSatClockCor(satid)-receiver_clock-vGroupDelay(satid);
%% Calculate the satellite position
[vSatPosNoRotation ,vSatClockCor(satid),vGroupDelay(satid)]= GetSatPosECEF(satid,vEpheEopchs(satid),time,transit_time );
%% Filter out the satellite with low elevation angle
vSatENU= ECEF2ENU(vSatPosNoRotation,vInitPos');
vElevationAngle(i) = atan(vSatENU(3)/norm(vSatENU(1:2)))*180/pi;
if (vElevationAngle(i) < maskangle) && (epoch~=1),
low_angle_id=satid;
%% Delete this satellite from the matrix recording
%% satellite PRNs, this satellite will then not be used
%% any more in the following parts.
mSat(1,satid,epoch)=0;
continue; %% go to process the next satellite
end
%% Correct the code data by removing receiver clock error,
%% satellite clock error, group delay
%% For smoothed and raw code data
vCorrectedCodeSmoothed(satid)=vCodeSmoothedCur(satid)-receiver_clock*sol+...
vSatClockCor(satid)*sol-vGroupDelay(satid)*sol;
vCorrectedCode(satid)=vCodeCur(satid)-receiver_clock*sol+...
vSatClockCor(satid)*sol-vGroupDelay(satid)*sol;
%% Implement the earth rotation to the satellite
vSatXYZ=EarthRotation(transit_time,vSatPosNoRotation);
%% Save the satellite positions of this epoch for single point posiitoning
mSatPosCur(satid,1:3)= vSatXYZ;
%% Save the satellite positions for future differential positioning
mSatPos(1,epoch,satid,1:3)= vSatXYZ;
end %% End of loop for satellites
%% Satellites used for the positioning at current epoch
vSatUsed=find(squeeze(squeeze(mSat(1,:,epoch)))~=0);
%% For master antenna, choose the key satellite for future DGPS
%% Key satellite implies the satellite having the highest
%% elevation angle herein.
%% For the post-processing and for the ambiguity resolution, the
%% key satellite could also be chosen as the satellite having the
%% longest visibility within a specific time span.
max_elevation=max(vElevationAngle);
vKeySat(epoch)=vSatIDs(find(vElevationAngle==max_elevation));
%% Single point positioning using smoothed code data
[vAntPos_sm,receiver_clock]=SinglePointGPS(vCorrectedCodeSmoothed,mSatPosCur,vSatUsed,vInitPos);
mAntXYZAll(1,epoch,1:3)=vAntPos_sm;
str=sprintf('Single point positioning %2.0f%%%', epoch/totalepoch*100);
waitbar(epoch/totalepoch,waitbar_sp,str)
end %% End of loop for epochs
close (waitbar_sp)
%% ********************************************************************
%% Differential GPS positioning
%% ********************************************************************
waitbar_DGPS = waitbar(0,'Differential Positioning');
for epoch=1:1:totalepoch, %% Loop for epoch
%% ~~~~~~~~~~~~~~~~ ~~~~~~~~~~~~~~~~~~~~~~
%% Get the corresponding data of the master antenna
%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%% Data of master antenna at this epoch
vSatIDMaster=find(squeeze(squeeze(mSat(1,:,epoch)))~=0);
vCodeMaster=mCode(1,:,epoch);
vPhaseMaster=mPhase(1,:,epoch);
vCodeSmoothedMaster=mCodeSmoothed(1,:,epoch);
mSatPosMaster=squeeze(mSatPos(1,epoch,:,1:3));
%% Estimated positiong of master antenna from single point positioning
vMasterPosition=squeeze(mAntXYZAll(1, epoch,1:3));
for antid=2:1:totalantenna,
%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%% Get the data from slave antenna
%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
clear mSlaveSatID vElevationAngle
%% Satellite IDs for slave antenna given in a compact format
vSatIDSlave=find(squeeze(squeeze(mSat(antid,:,epoch)))~=0);
%% Code data of slave antenna (compact)
vCodeSlave=mCode(antid,:,epoch);
vPhaseSlave=mPhase(antid,:,epoch);
vCodeSmoothedSlave=mCodeSmoothed(antid,:,epoch);
%% Search for the common satellites of master and this slave antenna
vCommonSatID= intersect(vSatIDSlave,vSatIDMaster);
satnum_common=length(vCommonSatID);
%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%% Calculate the satellite position for slave antenna
%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
for i=1:1:satnum_common,
satid=vCommonSatID(i); %% Satellite PRN
%% Find a proper ephemerides paragraph
time=mCommonEpoch(epoch); %% "Second of the week " of current epoch
eph_epoch=SeekEpheEpoch(time,satid);
%% "True" distance for master antenna
dis_true_master=norm(vMasterPosition-mSatPosMaster(satid,:)');
%% "True" distance for slave antenna
dis_true_slave=dis_true_master-vCodeMaster(satid)+vCodeSlave(satid);
%% Estimate the transit time of GPS signal.
transit_time=dis_true_slave/sol;
%% Calculate the satellite position
[vSatPosNoRotation ,sat_clock_err,sat_group_delay]= GetSatPosECEF(satid,eph_epoch,time,transit_time );
%% Implement the earth rotation to the satellite
vSatXYZ=EarthRotation(transit_time,vSatPosNoRotation);
%% Satellite position of slave antenna
mSatPosSlave(satid,:)=vSatXYZ;
end
%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%% Find the data from common satellites to construct baselines
%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%% Get the key satellite ID for the construction of differential
%% observation equation
key_sat_id=vKeySat(epoch);
%% Pick up the common visible satellites for DGPS processing
mCommonSatPosSlave=zeros(totalGPSsatellite,3);
mCommonSatPosMaster=zeros(totalGPSsatellite,3);
mCommonSatPosSlave(vCommonSatID,:)=mSatPosSlave(vCommonSatID,:);
mCommonSatPosMaster(vCommonSatID,:)=mSatPosMaster(vCommonSatID,:);
%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%% Get the ambiguity
%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
vN= mAmbiguity(antid-1,:);
%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%% DGPS solution
%%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
vSlavePosition_phase =DGPS_phase(mCommonSatPosMaster,mCommonSatPosSlave,vPhaseMaster,....
vPhaseSlave, key_sat_id,vCommonSatID,vN,vMasterPosition );
%% Record the slave antenna coordinates and baselines
%% for further attitude determination processing
mAntXYZAll(antid,epoch,1:3)=vSlavePosition_phase;
if num_baseline~=0,
%% Calculate the 3D error in the estimated baselines.
if antid==2,
baseline_true=b12;
elseif antid==3,
baseline_true=b13;
elseif antid>3,
baseline_true=mBaselineAdd(antid-3,1);
end
mBaselineEst(epoch,antid)=norm(vSlavePosition_phase-vMasterPosition');
mBaselineErr( epoch,antid )=mBaselineEst(epoch,antid)-baseline_true;
end
%% Save the local level coordinates of the slave antenna
mAntENUAll(antid,epoch,1:3)=ECEF2ENU(vSlavePosition_phase,vMasterPosition);
end
str=sprintf('Differential GPS processing %2.0f%%%', epoch/totalepoch*100 );
waitbar(epoch/totalepoch,waitbar_DGPS,str)
end %% End DGPS
close (waitbar_DGPS)
%% Since the results of 1st epoch are always of low quality, we therefore
%% delete this value.
if num_baseline~=0,
mBaselineEst(1,:)=[];
mBaselineErr(1,:)=[];
end
mAntXYZAll(:,1,:)=[];
mAntENUAll(:,1,:)=[];
totalepoch=totalepoch-1;
%% If baselines are identified, then display the baseline error
if num_baseline~=0,
%% Show baselines errors
for antid=2:1:totalantenna,
vBaselineErr=mBaselineErr(:,antid);
title_str=sprintf('Estimated baseline 3D error between antenna 1 and %d',antid);
DisplayBaselineErr(vBaselineErr,title_str);
end
end
%% ###################################################
%% ###################################################
%% Attitude determination
%% ###################################################
%% ###################################################
%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%% Direct computation
%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
waitbar_attitude1=waitbar(0,'Direct attitude computation');
for epoch=1:1:totalepoch,
%% Local level frame for slave antennas
%% only consider the antenna 1-3 and ignore the others
for antid=1:1:3,
mAntLocal(antid,1:3)=squeeze(mAntENUAll(antid,epoch,1:3));
end
%% Attitude determination by direct computation
%% Antenna 1 is [0 0 0]
%% Use only the local level coordinate of antenna 2 and 3
[yaw_deg_direct,roll_deg_direct,pitch_deg_direct]=AD_Direct(mAntLocal(2,:),mAntLocal(3,:));
%% Save the data
mAttitudeRecordDirect(epoch,1:3)=[yaw_deg_direct roll_deg_direct pitch_deg_direct ];
str=sprintf('Direct attitude computation %2.0f%%%', epoch/totalepoch*100);
waitbar(epoch/totalepoch,waitbar_attitude1,str)
end
close (waitbar_attitude1)
%% Demonstrate the result
DrawAttitudeFigure(mAttitudeRecordDirect, 'Attitude parameters based on direct computation')
SaveResultInFile(mAttitudeRecordDirect,mEpochStr, 'Result from direct attitude determination using CARRIER PHASE','Results.txt')
%% Once no baseline identified, then exit.
if num_baseline==0,
!results.txt;
return;
end
%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%% Attitude computation by LSQ
%% ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
%% ~~~~~ Step 1: Calculate the antenna body frame ~~~~~
mAntBody=GetBodyCoordinates(b12,b13,b23,mBaselineAdd);
%% Knowing only the magnitude of baselines we can not specify the Z value
%% of the antenna body frame for the antenna 4,5,6. We should employ the
%% calculated attitude parameters and the local level coordinate to derive
%% the estimated antenna body frame. The estimated antenna body frame will
%% help the determination of the Z value.
if totalantenna>=4,
%% Choose the first 50 epoch to calculate the antenna body frame based on
%% the attitude parameters obtained from the direct computation
if totalepoch>50, testepoch=50; else testepoch=totalepoch; end
%% Now get the sign of the Z value in the antenna body frame by using the
%% relationship: Body Frame=Rotation matrix* Local level frame, where the
%% rotation matrix is obtained from the attitude parameters based on the
%% direct attitude computation.
for epoch=1:1:testepoch,
mAntLocal =squeeze(mAntENUAll(:,epoch,1:3));
mR=GetCombinedRotationMatrix(mAttitudeRecordDirect(epoch,1),...
mAttitudeRecordDirect(epoch,2),mAttitudeRecordDirect(epoch,3));
mBodyTemp=(mR*mAntLocal')';
for i=4:1:totalantenna,
mSign(epoch,i)=mBodyTemp(i,3)/abs(mBodyTemp(i,3));
end
end
vSumSign=sum(mSign);
%% If 80% of the Z values are of the same sign, we then consider it as the
%% correct sign. Otherwise the user has to specify the sign(s).
for antid=4:1:totalantenna,
if abs(vSumSign(antid)/testepoch)>0.8,
sign_z=vSumSign(antid)/abs(vSumSign(antid));
mAntBody(antid,3)=abs(mAntBody(antid,3))*sign_z;
else
str=sprintf('Can not determine the Z value of antenna body frame of antenna %d, use default value instead',antid);
msgbox(str);
end
end
end
%% ~~~~~ Step 2: Start attitude calculation LSQ ~~~~~
waitbar_attitude2 = waitbar(0,'Attitude determination');
for epoch=1:1:totalepoch,
%% Local level frame for slave antennas
mAntLocal =squeeze(mAntENUAll(:,epoch,1:3));
%% Attitude determination by applying LSQ
[yaw_deg_lsq,roll_deg_lsq,pitch_deg_lsq]=AD_LSQ(mAntLocal ,mAntBody ,0,0,0);
%% Save the data
mAttitudeRecordLSQLinear(epoch,1:3)=[yaw_deg_lsq roll_deg_lsq pitch_deg_lsq];
str=sprintf('Least squares attitude determination %2.0f%%%', epoch/totalepoch*100);
waitbar(epoch/totalepoch,waitbar_attitude2,str)
end
%%
close (waitbar_attitude2)
DrawAttitudeFigure(mAttitudeRecordLSQLinear,'Attitude parameters based on linearized least squares adjustment')
SaveResultInFile(mAttitudeRecordLSQLinear,mEpochStr, 'Result from least squares estimation using CARRIER PHASE','Results.txt')
type('results.txt');